function [FINSSysBlk] = naverrdynmat_inssysblk_phiform(CNE, vN, fN, L, h, G, FCN, C, omegaIE, navCoordType, accoutForGravErr, INSSysStateMask) %#codegen
%NAVERRDYNMAT_INSSYSBLK_PHIFORM 计算φ形式的导航误差方程对应的误差动态矩阵的系统状态→系统状态块
%
% Input Arguments:
% # CNE: 3*3矩阵，位置矩阵，无单位
% # vN: 3*1向量，速度，单位m/s
% # fN: 3*1向量，比力，单位m/s^2
% # L: 标量，纬度，单位rad
% # h: 标量，高度，单位m
% # G: 标量，引力加速度，单位m/s^2
% # FCN: 3*3矩阵，曲率矩阵，无单位
% # C: 1*3向量，高度通道反馈增益系数，分别反馈至速度二次微分、速度微分、高度微分，均为0或为空时屏蔽高度通道阻尼
% # omegaIE: 标量，地球自转角速率，单位rad/s
% # navCoordType: 枚举标量，NavCoordType.GEO为ENU地理坐标系，NavCoordType.WAN_AZM为游动方位坐标系，NavCoordType.FREE_AZM为自由方位坐标系
% # accoutForGravErr: 逻辑标量，true表示考虑重力误差
% # INSSysStateMask: 1*11逻辑向量，true表示包含该系统状态，false表示不包含该系统状态，详细状态为第1、2、10列元素为位置误差，第3列元素为高度误差，第4-6列元素为速度误差，第7-9列元素为姿态误差，第11列为deVC3
%
% Output Arguments:
% # FINSSysBlk: m*m矩阵，m为INSSysStateMask中为真的个数，误差动态矩阵的系统状态→系统状态块，无单位
%
% Assumptions and Limitations:
% # 理论上，传入本函数的导航参数应为理想值
%
% References:
% # 《应用导航算法工程基础》“导航系下观测的误差投影在导航系下的参数组”

%% 预计算
tanL = tan(L);
tanLSquarePlusOne = tanL^2 + 1;
RMPlushReciprocal = coder.nullcopy(NaN);
RNPlushReciprocal = coder.nullcopy(NaN);
RMPlushRecSquare = coder.nullcopy(NaN);
RNPlushRecSquare = coder.nullcopy(NaN);
R0PlushReciprocal = coder.nullcopy(NaN);
R0PlushRecSquare = coder.nullcopy(NaN);
if navCoordType == NavCoordType.GEO
    % REF1式9.105
    RMPlushReciprocal = FCN(1, 1);
    RNPlushReciprocal = FCN(2, 2);
    RMPlushRecSquare = RMPlushReciprocal^2;
    RNPlushRecSquare = RNPlushReciprocal^2;
else
    % REF1第9.1.6节
    R0PlushReciprocal = (FCN(1, 1) + FCN(2, 2))/2;
    R0PlushRecSquare = R0PlushReciprocal^2;
end

%% 状态块中各分量式
omegaIEN = earthrate(CNE, omegaIE);
omegaENN = FCN * [-vN(2, 1) vN(1, 1) 0]'; % 等号右边后一项为[0 0 1]'×vN
if navCoordType == NavCoordType.GEO
    omegaENN(3, 1) = tanL * omegaENN(2, 1);
elseif navCoordType == NavCoordType.FREE_AZM
    omegaENN(3, 1) = -omegaIEN(3, 1);
end
omegaIENx = omegaIEN(1, 1); 
omegaIENy = omegaIEN(2, 1);
omegaIENz = omegaIEN(3, 1);
omegaENNx = omegaENN(1, 1);
omegaENNy = omegaENN(2, 1);
omegaENNz = omegaENN(3, 1);
vNx = vN(1, 1);
vNy = vN(2, 1);
vNz = vN(3, 1);
fNx = fN(1, 1);
fNy = fN(2, 1);
fNz = fN(3, 1);

%% 计算各状态块
FINSSysBlkFull = zeros(11);
switch navCoordType
    case NavCoordType.GEO
        FINSSysBlkFull(1:10, 1:10) =    [                                                                0,        omegaENNz,                                     vNy*RMPlushRecSquare,                                                 0,              -1*RMPlushReciprocal,                         0,                       0,                     0,                       0,       -omegaENNy ;
                                                                                    -omegaENNz,                0,                                    -vNx*RNPlushRecSquare,                                        1*RNPlushReciprocal,                        0,                         0,                       0,                     0,                       0,        omegaENNx ;
                                                                                             0,                0,                                                  0,                                                 0,                        0,                         1,                       0,                     0,                       0,                0 ;
                              - 2*omegaIENz*vNz - vNy*(2*omegaIENy + omegaENNy*tanLSquarePlusOne),                0, (vNx*vNz)*RNPlushRecSquare - (vNx*vNy*tanL)*RNPlushRecSquare,              (vNy*tanL)*RNPlushReciprocal - vNz*RNPlushReciprocal,  2*omegaIENz + omegaENNz, - 2*omegaIENy - omegaENNy,                       0,                  -fNz,                     fNy,                0 ;
                                                  vNx*(2*omegaIENy + omegaENNy*tanLSquarePlusOne), -2*omegaIENz*vNz,   (vNx^2*tanL)*RNPlushRecSquare + (vNy*vNz)*RMPlushRecSquare, - 2*omegaIENz - omegaENNz - (vNx*tanL)*RNPlushReciprocal,            -vNz*RMPlushReciprocal,                 omegaENNx,                     fNz,                     0,                    -fNx,  2*omegaIENy*vNz ;
                                                                               2*omegaIENz*vNx,  2*omegaIENz*vNy,              - vNy^2*RMPlushRecSquare - vNx^2*RNPlushRecSquare,            2*omegaIENy + omegaENNy + vNx*RNPlushReciprocal, vNy*RMPlushReciprocal - omegaENNx,                         0,                    -fNy,                   fNx,                       0, -2*omegaIENy*vNy ;
                                                                                             0,       -omegaIENz,                                     vNy*RMPlushRecSquare,                                                 0,              -1*RMPlushReciprocal,                         0,                       0, omegaIENz + omegaENNz, - omegaIENy - omegaENNy,        omegaIENy ;
                                                                                     omegaIENz,                0,                                    -vNx*RNPlushRecSquare,                                        1*RNPlushReciprocal,                        0,                         0, - omegaIENz - omegaENNz,                     0,               omegaENNx,                0 ;
                                                        - omegaIENy - omegaENNy*tanLSquarePlusOne,                0,                           -(vNx*tanL)*RNPlushRecSquare,                                   tanL*RNPlushReciprocal,                        0,                         0,   omegaIENy + omegaENNy,            -omegaENNx,                       0,                0 ;
                                                          omegaENNy - omegaENNy*tanLSquarePlusOne,       -omegaENNx,                           -(vNx*tanL)*RNPlushRecSquare,                                   tanL*RNPlushReciprocal,                        0,                         0,                       0,                     0,                       0,                0];
    case NavCoordType.WAN_AZM
        FINSSysBlkFull(1:10, 1:10) =    [                                   0,                                   0,                        vNy*R0PlushRecSquare,                                      0,                            -1*R0PlushReciprocal,                         0,                     0,                       0,                       0,                          -omegaENNy ;
                                                                0,                                   0,                       -vNx*R0PlushRecSquare,                             1*R0PlushReciprocal,                                      0,                         0,                     0,                       0,                       0,                           omegaENNx ;
                                                                0,                                   0,                                     0,                                      0,                                      0,                         1,                     0,                       0,                       0,                                   0 ;
                              - 2*omegaIENy*vNy - 2*omegaIENz*vNz,                     2*omegaIENx*vNy,                  (vNx*vNz)*R0PlushRecSquare,                          -vNz*R0PlushReciprocal,                            2*omegaIENz, - 2*omegaIENy - omegaENNy,                     0,                    -fNz,                     fNy,                     2*omegaIENx*vNz ;
                                                  2*omegaIENy*vNx, - 2*omegaIENx*vNx - 2*omegaIENz*vNz,                  (vNy*vNz)*R0PlushRecSquare,                           -2*omegaIENz,                          -vNz*R0PlushReciprocal,   2*omegaIENx + omegaENNx,                   fNz,                       0,                    -fNx,                     2*omegaIENy*vNz ;
                                                  2*omegaIENz*vNx,                     2*omegaIENz*vNy, - vNx^2*R0PlushRecSquare - vNy^2*R0PlushRecSquare, 2*omegaIENy + omegaENNy + vNx*R0PlushReciprocal, vNy*R0PlushReciprocal - omegaENNx - 2*omegaIENx,                         0,                  -fNy,                     fNx,                       0, - 2*omegaIENx*vNx - 2*omegaIENy*vNy ;
                                                                0,                          -omegaIENz,                        vNy*R0PlushRecSquare,                                      0,                            -1*R0PlushReciprocal,                         0,                     0,               omegaIENz, - omegaIENy - omegaENNy,                           omegaIENy ;
                                                        omegaIENz,                                   0,                       -vNx*R0PlushRecSquare,                             1*R0PlushReciprocal,                                      0,                         0,            -omegaIENz,                       0,   omegaIENx + omegaENNx,                          -omegaIENx ;
                                                       -omegaIENy,                           omegaIENx,                                     0,                                      0,                                      0,                         0, omegaIENy + omegaENNy, - omegaIENx - omegaENNx,                       0,                                   0 ;
                                                        omegaENNy,                          -omegaENNx,                                     0,                                      0,                                      0,                         0,                     0,                       0,                       0,                                   0];
    case NavCoordType.FREE_AZM
        FINSSysBlkFull(1:10, 1:10) =    [                                 0,                        -omegaIENz,                        vNy*R0PlushRecSquare,                                      0,                            -1*R0PlushReciprocal,                         0,                     0,                       0,                       0,                          -omegaENNy ;
                                                      omegaIENz,                                 0,                       -vNx*R0PlushRecSquare,                             1*R0PlushReciprocal,                                      0,                         0,                     0,                       0,                       0,                           omegaENNx ;
                                                              0,                                 0,                                     0,                                      0,                                      0,                         1,                     0,                       0,                       0,                                   0 ;
                              - omegaIENy*vNy - 2*omegaIENz*vNz,                     omegaIENx*vNy,                  (vNx*vNz)*R0PlushRecSquare,                          -vNz*R0PlushReciprocal,                              omegaIENz, - 2*omegaIENy - omegaENNy,                     0,                    -fNz,                     fNy,                     2*omegaIENx*vNz ;
                                                  omegaIENy*vNx, - omegaIENx*vNx - 2*omegaIENz*vNz,                  (vNy*vNz)*R0PlushRecSquare,                             -omegaIENz,                          -vNz*R0PlushReciprocal,   2*omegaIENx + omegaENNx,                   fNz,                       0,                    -fNx,                     2*omegaIENy*vNz ;
                                                2*omegaIENz*vNx,                   2*omegaIENz*vNy, - vNx^2*R0PlushRecSquare - vNy^2*R0PlushRecSquare, 2*omegaIENy + omegaENNy + vNx*R0PlushReciprocal, vNy*R0PlushReciprocal - omegaENNx - 2*omegaIENx,                         0,                  -fNy,                     fNx,                       0, - 2*omegaIENx*vNx - 2*omegaIENy*vNy ;
                                                              0,                        -omegaIENz,                        vNy*R0PlushRecSquare,                                      0,                            -1*R0PlushReciprocal,                         0,                     0,                       0, - omegaIENy - omegaENNy,                           omegaIENy ;
                                                      omegaIENz,                                 0,                       -vNx*R0PlushRecSquare,                             1*R0PlushReciprocal,                                      0,                         0,                     0,                       0,   omegaIENx + omegaENNx,                          -omegaIENx ;
                                                              0,                                 0,                                     0,                                      0,                                      0,                         0, omegaIENy + omegaENNy, - omegaIENx - omegaENNx,                       0,                                   0 ;
                                          omegaIENy + omegaENNy,           - omegaIENx - omegaENNx,                                     0,                                      0,                                      0,                         0,                     0,                       0,                       0,                                   0];
    otherwise
        FINSSysBlkFull = NaN(11); %#ok<NASGU>
        error('naverrdynmat_inssysblk_phiform:navCoordType', 'Invalid navigation coordinate type.');
end
if any(C) 
    % 考虑垂直通道阻尼
    FINSSysBlkFull(3, 3) = FINSSysBlkFull(3, 3) - C(1, 3);
    FINSSysBlkFull(6, 3) = FINSSysBlkFull(6, 3) - C(1, 2);
    FINSSysBlkFull(11, 3) = C(1, 1);
    FINSSysBlkFull(6, 11) = -1;
end
if accoutForGravErr
    % 考虑重力误差
    % REF1式9.72
    if h>=0
        Fh = 2;
    else
        Fh = -1;
    end    
    FINSSysBlkFull(6, 3) = FINSSysBlkFull(6, 3) + Fh*G*(FCN(1, 1)+FCN(2, 2))/2; % NOTE: 此处用R0+h近似代替载体所在处的地球半径R
end
FINSSysBlk = FINSSysBlkFull(INSSysStateMask, INSSysStateMask);
end